#include "pid.h"

double UpdatePID(SPid * pid, double value, uint32_t* Ticks, double error, double position)
{
	double pTerm, dTerm, iTerm, tTerm;
	pTerm = pid->pGain * error;
	// calculate the proportional term
	// calculate the integral state with appropriate limiting
	pid->iState += error;

	if (pid->iState > pid->iMax)
		pid->iState = pid->iMax;
	else if (pid->iState < pid->iMin)
		pid->iState = pid->iMin;

	iTerm = pid->iGain * pid->iState;  // calculate the integral term
	dTerm = pid->dGain * (position - pid->dState);

	tTerm = 0; // Initialise the tTerm 
	if (pid->underValue > pid->overValue) // Perform a bounce check 
	{ 
		pid->overValue = pid->underValue; 
	} 
	if (error > 0) // Negative feedback required 
	{ 
		if (value < pid->underValue)
			pid->underValue = value;

		if (pid->dError <= 0) // Record Overshoot (value and time) 
		{ 
			pid->overValue = value; 
			pid->overTime = *Ticks; 
		} 

		pid->tDiff = *Ticks - pid->overTime;
		if (pid->tDiff > pid->minTime) // Perform the calculation 
		{ 
			tTerm = (pid->tDiff - pid->minTime) * pid->tGain * (pid->overValue - pid->underValue); 
		} 
	} 
	if (error < 0) // Positive feedback required 
	{ 
		if (value > pid->overValue)
			pid->overValue = value;

		if (pid->dError >= 0) // Record the Undershoot (value and time) 
		{ 
			pid->underValue = value; 
			pid->underTime = *Ticks; 
		} 

		pid->tDiff = *Ticks - pid->underTime;
		if (pid->tDiff > pid->minTime) // Perform the calculation 
		{ 
			tTerm = (pid->tDiff - pid->minTime) * pid->tGain * (pid->overValue - pid->underValue) * -1; 
		} 
	} 

	pid->dError = error;    // Save error for next time 
	pid->dState = position;
	// Calculate the time adjuster
	return pTerm + iTerm - dTerm + tTerm;
}
